#!/usr/bin/python3
# -*- coding: utf-8 -*-


from cmath import inf
import rospy
from tf.msg import tfMessage
import time
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import MultiEchoLaserScan
from sensor_msgs.msg import LaserEcho 


class A():
  def __init__(self) -> None:
    self.pub = rospy.Publisher('/horizontal_laser_2d', MultiEchoLaserScan, queue_size=1)
    self.val = MultiEchoLaserScan()

    self.if_print = True
    self.data_LaserEcho = LaserEcho()

  def callback(self, data):
    t_begin = time.time()
    self.val.header = data.header
    self.val.header.frame_id = "horizontal_laser_link"
    # print(rospy.Time.now().to_sec())
    # print(rospy.Time.now().secs)
    # print(rospy.Time.now().nsecs)
    # t = rospy.Time.from_sec(rospy.Time.now())
    
    # self.val.header.stamp.secs = rospy.Time.now().secs
    # self.val.header.stamp.nsecs = rospy.Time.now().nsecs.
    # self.val.header.stamp.secs = self.val.header.stamp.secs-252777018
    # print(self.val.header.stamp.secs, '000000000000')
    self.val.angle_min = data.angle_min
    self.val.angle_max = data.angle_max
    self.val.angle_increment = data.angle_increment
    self.val.time_increment = data.time_increment
    self.val.scan_time = data.scan_time
    self.val.range_min = data.range_min
    self.val.range_max = data.range_max
    self.val.ranges = []
    self.val.intensities = []

    # 接下来处理ranges和intensities
    for d in data.ranges:
      if d == inf:
        d = data.range_max
      dt = LaserEcho()
      dt.echoes.append(d)
      self.val.ranges.append(dt)
    for d in data.intensities:
      if d == inf:
        d = data.range_max
      dt = LaserEcho()
      dt.echoes.append(d)
      self.val.intensities.append(dt)

    if self.if_print:
      # print(self.val)
      self.if_print = False
    
    # 发布
    self.pub.publish(self.val)
    print(time.time()-t_begin)

def main():
  print('begin')
  rospy.init_node('change_bag_laser')
  a = A()
  rospy.Subscriber('/scan', LaserScan, a.callback)
  rospy.spin()

if __name__ == '__main__':
  main()
